from pb.ros.sensor_msgs import Image_pb2
from pb.ros.geometry_msgs import Vector3_pb2
import time
import numpy as np
import cv2.cv2 as cv2
import proxy

B = 0


def CB_IMAGE(image):
    global B

    E = time.time()
    imagedata = np.frombuffer(image.data, np.uint8)
    height, width = image.height, image.width

    img_rgba = imagedata.reshape(image.height, image.width, 4)
    img_brg = cv2.cvtColor(img_rgba, cv2.COLOR_RGBA2BGR)

    print(E - B)  # 接收间隔
    B = E

    cv2.imshow("Temporary Image", img_brg)
    cv2.waitKey(1)


# 请优先使用高带宽网络
# node_ = proxy.Node("Case2", "tcp://10.10.10.205:6010", "tcp://10.10.10.205:7009") # 内网ＩＢ
# node_ = proxy.Node("Case2", "tcp://12.12.12.205:6010", "tcp://12.12.12.205:7009") # 内网万兆
# node_ = proxy.Node("dev", "tcp://172.22.56.13:6006", "tcp://172.22.56.13:7006") # 外网千兆
node_ = proxy.Node("dev", "tcp://192.168.88.88:6658", "tcp://192.168.88.88:6587")  # 外网千兆

# subscriber = node_.Subscribe('Case2', '/Case2/M300_0/color/image_raw', Image_pb2.Image, CB_IMAGE) # M300_1, M300_2； M300_0, FW01_0
subscriber = node_.Subscribe('dev', '/dev/M300_0/color/image_raw', Image_pb2.Image,
                             CB_IMAGE)  # M300_1, M300_2； M300_0, FW01_0

if False == subscriber.IsVaild():
    print(" subscriber 初始化失败")
    subscriber.UnSubscribe()

# 下面四种方法都是检查数据是否已经到达并且主动调用回调函数
# 根据实际情况选择一种方法

# 1. 不管数据是否已经到达都马上返回，如果数据已经到达，即引起回调函数的调用，否则什么都不做直接返回
# subscriber_.SpinOnce();

# 2.检查一次数据是否已经到,直到数据到达后才返回
# subscriber_.SpinFor();

# 3.检查一次数据是否已经到,直到数据或超时，数据到达后才返回，单位为：毫秒
# 等待一秒
# subscriber_.SpinFor(1000);

# 4.Spin方法，一直循环检查数据是否已经到，通知订阅回调，永不返回，直到用户调用UnSubscribe()方法
subscriber.Spin()